Complete Closed-Form and Accurate Solution to Pose Estimation From 3D Correspondences

Computing the pose from 3D data acquired in two different frames is of high importance for several robotic tasks like odometry, SLAM and place recognition. The generally obtained by solving a least-squares problem given points-to-points, points-to-planes or points to lines correspondences. non-linear can be solved iterative optimization or, more efficiently, closed-form using solvers polynomial systems. In this letter, complete accurate solution weighted proposed. Adding weights each correspondence allow increase robustness outliers. Contrary existing methods, proposed approach since it able solve any non-degenerate case guaranteed find global optimal estimate problem. Simulations experiments on real demonstrate superior accuracy algorithm compared previous approaches.